#include "patrol.hpp"
#include "posinfo.hpp"

Patrol::Patrol()
{
    /* 解析导航点信息 */
    coords = parse("/unitree/module/graph_pid_ws/src/QT_Server/graph_map/Make_Graph/Floor_0/pcd_0/Make_Graph_node_0_0_0.txt");

    /* 打印解析得到的导航点信息 */
    if (coords.empty()) {
        std::cerr << "can not parse the coords." << std::endl;
    } else {
        for (const auto& coord : coords) {
            std::cout << "x: " << coord.x << ", y: " << coord.y << ", yaw: " << coord.yaw << std::endl;
        }
    }
}

Patrol::~Patrol()
{
}

void Patrol::thread()
{
    bool valid;
    float x, y, yaw;

    /* 等待机器狗雷达定位成功 */
    while (running) {
        PosInfo::get(valid, x, y, yaw);
        if (false == valid) {
            msleep(1000);
            continue;
        }

        std::cout << "Patrol, " << valid << " point : " << x << ", " << y << ", " << yaw << std::endl;
        break;
    }

    /* 检查是否有导航点信息 */
    int count = coords.size();
    if (count == 0) {
        std::cerr << "coords count is 0." << std::endl;
        return;
    }
    std::cout << "coords count is " << count << std::endl;

    /* 从 index = 0 位置开机，因此第一个目标是 index = 1 位置 */
    int index = 1;
    while (running) {
        /* 获取机器狗目标位置 */
        Coordinate coord = coords[index];
        std::cout << "Patrol, target point " << index << ": " << coord.x << ", " << coord.y << ", " << coord.yaw << " --- " << index << std::endl;

        motion.sport_position(coord.x, coord.y, coord.yaw);

        index = index + 1;
        if (index >= count) {
            index = 0;
        }

        msleep(40);
    }
}

void Patrol::start()
{
    if (!handle.joinable()) {
        running = true;
        handle = std::thread(&Patrol::thread, this);
    }
}

void Patrol::stop()
{
    if (handle.joinable()) {
        running = false;
        handle.join();
    }
}

void Patrol::msleep(int ms)
{
    usleep(ms * 1000);
}

std::vector<Coordinate> Patrol::parse(const std::string& filename)
{
    std::vector<Coordinate> coordinates;
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法打开文件！" << std::endl;
        return coordinates;
    }

    std::string line;
    while (std::getline(file, line)) {
        std::istringstream iss(line);
        int index;
        double col1, col2, col3, col4;

        // 跳过第一列
        iss >> index;

        // 读取第二列 (x)
        iss >> col1;

        // 读取第三列 (y)
        iss >> col2;

        // 跳过第四列
        iss >> col3;

        // 读取第五列 (yaw)
        iss >> col4;

        Coordinate coord;
        coord.x = col1;
        coord.y = col2;
        coord.yaw = col4;
        coordinates.push_back(coord);
    }

    file.close();
    return coordinates;
}